#ifndef _MOTOR_H__
#define _MOTOR_H__
#include "os/types.h" // IWYU pragma: export
#include "controller/controller.h"
#include "controller/hw_samples.h"
#include "controller/encoder.h"
#include "utils/utils.h"
#include "bsp/bsp.h"


#define PHASEA_CURR_IDX 0
#define PHASEB_CURR_IDX 1
#define PHASEC_CURR_IDX 2
#define PHASEA_VOL_IDX 3
#define PHASEB_VOL_IDX 4
#define PHASEC_VOL_IDX 5
#define INPUT_TORQUE_IDX 6
#define OUTPUT_TORQUE_IDX 7
#define INPUT_VELOCITY_IDX 8
#define ENCODER_VELOCITY_IDX 9
#define ENCODER_ANGLE_IDX 10
#define SENSORLESS_ANGLE_IDX 11
#define SENSORLESS_VELOCITY_IDX 12
#define D_AXIS_CURR_IDX 13
#define Q_AXIS_CURR_IDX 14
#define D_AXIS_VOL_IDX 15
#define Q_AXIS_VOL_IDX 16
#define D_TARGET_CURR_IDX 17
#define Q_TARGET_CURR_IDX 18
#define SVPWM_DUTYA_IDX 19
#define SVPWM_DUTYB_IDX 20
#define SVPWM_DUTYC_IDX 21
#define DC_CURRENT_IDX 22
#define AGNLE_DIFF_IDX 23

#define PLOT_MAX_IDS   4


typedef struct {
	bool   			b_start;
	u8     			mode_running;
	float   		torque_command;
	float   		velocity_command;
	float           vdq_command[2];
	float           currdq_command[2];
	u8      		plot_ids[PLOT_MAX_IDS];
	controller_t 	controller;
	hw_samples_t    samples;
	mag_encoder_t 	encoder;
	os_timer_t      force_timer;
	/* for debug adc sample time */
	u32             adc_cnt;
	u32 			adc_count[2];
	/* foc exec time, and interval time*/
	u32        		foc_bench;
	u32        		delta_ticks;
	/* mc schel exec time */
	u32        		slow_bench;

}motor_t;

void motor_init(motor_t *motor);
bool motor_start(motor_t *motor, u8 mode);
bool motor_stop(motor_t *motor);
void motor_adc_irq_handler(motor_t *motor);
void timer_up_irq_handler(motor_t *motor);
void mc_sched_irq_handler(motor_t *motor);
void mc_fault_irq_handler(motor_t *motor);
bool motor_force_foc_angle(motor_t *motor, float vd, float manu_vel);

extern motor_t g_motor[1];

__STATIC_INLINE motor_t *motor(int index) {
	return &g_motor[index];
}

__STATIC_INLINE controller_t *controller(motor_t *motor) {
	return &motor->controller;
}

__STATIC_INLINE void motor_set_target_velocity(motor_t *motor, float velocity) {
	motor->velocity_command = velocity;
}

__STATIC_INLINE void motor_set_target_torque(motor_t *motor, float torque) {
	motor->torque_command = torque;
}

__STATIC_INLINE void motor_set_target_vdq(motor_t *motor, float vd, float vq) {
	contrl_set_target_vdq(controller(motor), vd, vq);
}

__STATIC_INLINE void motor_set_target_currdq(motor_t *motor, float id, float iq) {
	motor->currdq_command[0] = id;
	motor->currdq_command[1] = iq;
}

__STATIC_INLINE void motor_init_encoder(motor_t *motor) {
#if (CONFIG_USE_MAG_ENCODER)
	encoder_init(&motor->encoder);	
#endif	
}

__STATIC_INLINE void motor_update_encoder(motor_t *motor) {
#if (CONFIG_USE_MAG_ENCODER)
	encoder_update_elec_angle(&motor->encoder);
#endif	
}

__STATIC_INLINE float motor_get_encoder_velocity(motor_t *motor) {
#if (CONFIG_USE_MAG_ENCODER)
	return encoder_get_velocity(&motor->encoder);
#else
	return sim_encoder_get_velocity();
#endif
}
	
__STATIC_INLINE float motor_get_encoder_angle(motor_t *motor) {
#if (CONFIG_USE_MAG_ENCODER)
	return encoder_get_elec_angle(&motor->encoder);
#else
	return sim_encoder_get_angle();
#endif
}

#endif /* _MOTOR_H__ */
